program program_psr
use madx_ptc_module
use pointer_lattice
implicit none

    interface
       subroutine build_PSR(PSR,MIS)
         use madx_ptc_module
         use pointer_lattice
         implicit none
         type(layout), target :: PSR
         logical(lp) mis
       end subroutine build_PSR
    end interface

type(layout), pointer:: psr
type(damapspin) id,m,a_t,a_f,a_s,a_l,a_nl
real(dp) closed_orbit(6),coeff(4),THETA0
type(probe) ray_closed
type(probe_8) ray
type(internal_state) state
type(normal_spin) normal
integer :: no1=0,np1=0 
type(taylor) x2,average_floquet,average_xp,radii(2),invariant(2)
logical(lp) mis
TYPE(spinor_8) N_AXIS

call ptc_ini

psr=>m_u%start

Write(6,*) " small misalignments and rotations in lattice ? input t or f "
read(5,*) mis

call build_PSR(PSR,mis)

Write(6,*) " Map with only 4d or with delta (4d+1)? "
Write(6,*) "Type t (true) for only 4d "
read(5,*) mis
if(mis) then
 !!!  Phase space of Taylor maps will be (x,px,y,py)
 state=only_4d0+spin0
else
 !!!  Phase space of Taylor maps will be (x,px,y,py,delta)  
 state=delta0+spin0
endif

do while(no1<=1) 
 write(6,*) " input an order > 1 (not too big first)! "
 read(5,*) no1
enddo

np1=0    !!!  No system parameters in the map

call init(state,no1,np1) ! PTC and FPP are properly initialized

call alloc(id,m,a_t,a_f,a_s,a_l,a_nl)  
call alloc(ray)
call alloc(radii)
call alloc(invariant)
call alloc(normal)
call alloc(x2,average_floquet,average_xp)
call alloc(N_AXIS)

closed_orbit=0.d0 ! initial guess for closed orbit

call find_orbit(psr,closed_orbit,1,state,1.d-5)

write(6,'(a16,4(1x,g12.5))') " closed orbit = ",closed_orbit(1:4)

RAY_CLOSED=closed_orbit
    CALL FIND_ORBIT_probe_spin(psr,RAY_CLOSED,STATE,c_1d_5,FIBRE1=1,theta0=theta0)
 

id=1;   ray=RAY_CLOSED+id;   ! ray= closed orbit + identity map

CALL TRACK_PROBE(PSR,RAY,STATE,FIBRE1=1) ! One turn map is computed via the ray

!!! The ray is really the result of a truncated power series algebra
!!! The map we are interested to compute is around the closed orbit: it is really part of a diffential algebra
m=ray   !   The ray is "officially" turned into a "damapspin"  (DA in Berz's honour)

normal = m  ! The map is normalised ORBITAL+SPIN   



!!! Same as the old code except that we must drag
!   the orbital part of the maps (damap out of damapspin)
!   if ID is a damapspin, then orbital part is in id%m
radii(1)=id%m%v(1)**2+id%m%v(2)**2 
radii(2)=id%m%v(3)**2+id%m%v(4)**2
!!! Same as the old code except that we must drag
!   the orbital part of the normal form (normalform out of normal_spin)
!   if normal is a normal_spin, then orbital part is in normal%n
invariant(1)=radii(1)*normal%n%A_T**(-1)
invariant(2)=radii(2)*normal%n%A_T**(-1)

call print(invariant(1),6)
call print(invariant(2),6)

!!! Now some spin stuff !!!
write(6,*) "nx,ny,nu_spin"
write(6,'(3(1x,E15.7))') normal%n%tune(1:2),normal%nu

a_t=normal%a_t
!! (a_t%m,a_t%s) = (I ,a_s%s) o (a_f%m, I ) o (a_l%m,I) o (a_nl%m,I)
call factor(a_t,a_s,a_f,a_l,a_nl)    




 ! The normal form is a collection of erect spinors s=(0,1,0)

n_axis=2           
Write(6,*) " Normal form : erect spin "
call print(n_axis,6)

n_axis=a_s*n_axis ! n=A_s(0,1,0)
 
Write(6,*) " Invariant Spin Field (ISF)"
call print(n_axis,6)

id=1;   ray=RAY_CLOSED+id;   ! ray= closed orbit + identity map

ray%s(1)=n_axis

CALL TRACK_PROBE(PSR,RAY,STATE,FIBRE1=1) ! Tracking the ISF using BMT
!!! ray%s(1) contains the transformed ISF

id=ray   
n_axis=n_axis*id%m  ! Transforming the ISF by the orbital map

n_axis%x(1)=n_axis%x(1)-ray%s(1)%x(1)
n_axis%x(2)=n_axis%x(2)-ray%s(1)%x(2)
n_axis%x(3)=n_axis%x(3)-ray%s(1)%x(3)
write(6,*) "Computing the difference between BMT on n and n o m "
call print(n_axis,6)
write(6,*) "   "
write(6,*) " hit return to terminate program "
write(6,*) "   "
pause 


end program program_psr


!=================================================================
subroutine  build_PSR(PSR,mis)
use madx_ptc_module
use pointer_lattice
implicit none

type(layout), target :: PSR

real(dp) :: ang, brho, kd, kf, Larc,SG(6),cut
type(fibre) :: b, d1, d2, qd, qf
type(layout) :: cell
logical(lp) mis
!-----------------------------------

call make_states(.false.)
exact_model = .true.
default = default + nocavity  
call update_states
madlength = .false.

ang = (twopi * 36.d0 / 360.d0)
Larc = 2.54948d0
brho = 1.2d0 * (Larc / ang)
call set_mad(brho = brho, method = 2, step = 10)
madkind2 = drift_kick_drift

kf =  2.72d0 / brho
kd = -1.92d0 / brho

d1 = drift("D1", 2.28646d0)
d2 = drift("D2", 0.45d0)
qf = quadrupole("QF", 0.5d0, kf)
qd = quadrupole("QD", 0.5d0, kd)
b  = rbend("B", Larc, ang)
cell = d1 + qd + d2 + b + d2 + qf + d1

PSR = 10 * cell
PSR = .ring.PSR

call survey(PSR)


if(mis) then
 sig=1.d-5
 cut=4.d0
 call MESS_UP_ALIGNMENT(PSR,SIG,cut)
endif
end subroutine build_PSR
